package simulator.implementation.agent;
import java.awt.image.BufferedImage;
import java.awt.Color;

import javax.vecmath.Vector3d;

import simbad.sim.CameraSensor;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simulator.meta.agent.ControlledAgent;
public class TestRobot extends ControlledAgent{
	//Constants:
	private final byte NUMBER_OF_ULTRASONIC_SENSORS = 10;
	private final boolean RED = true;
	private final boolean GREEN = false;
	//Attributes:
	private boolean mood;
	private RangeSensorBelt sonars;
	private CameraSensor camera;
	private BufferedImage cameraImage;
	//Constructor: 
	public TestRobot(Vector3d position, String name) {
		super(position, name);
		this.mood = this.RED;
		this.step = 0;
		this.turn = 0;
		this.sonars = RobotFactory.addSonarBeltSensor(this, this.NUMBER_OF_ULTRASONIC_SENSORS);
		//Añadir cámara:
		this.camera = RobotFactory.addCameraSensor(this);
		//Reservar memoria para la captura de imagen:
		this.cameraImage = this.camera.createCompatibleImage();
	}
	//Methods:
	public void initBehavior() {}

	public void performBehavior() {
		this.setTranslationalVelocity(this.step);
		this.setRotationalVelocity((Math.PI/180)*this.turn);
		
		this.camera.copyVisionImage(this.cameraImage);
		
		if(this.identifyObject(this.cameraImage,((this.mood==this.RED)?Color.RED:Color.GREEN))){
			this.setRotationalVelocity(0);
			this.setTranslationalVelocity(5);
			if(this.sonars.getMeasurement(0)<1){
				this.mood = !this.mood;
			}
		}else{
			this.setRotationalVelocity(0.5);
		}
	}
	private boolean identifyObject(BufferedImage image,Color target){
		for (int x = 0; x < image.getWidth(); x++) {//X
			for (int y = 0; y < image.getHeight(); y++) {//Y
				if(image.getRGB(x, y) == target.getRGB()){
					return true;
				}
			}
		}
		return false;
	}
	public void stepUp(){
		if(this.stepStatus!=this.GOING_FORTH&&this.step==(-1*this.STEP_INCREASE)){
			this.step=0;
			this.stepStatus = this.GOING_FORTH;
		}else{
			this.step+=(this.step>3)?0:this.STEP_INCREASE;
		}
	}
	public void stepDown(){
		if(this.stepStatus==this.GOING_FORTH&&this.step==this.STEP_INCREASE){
			this.step=0;
			this.stepStatus = !this.GOING_FORTH;
		}else{
			this.step-=(this.step<-3)?0:this.STEP_INCREASE;
		}
	}
	public void turnLeft(){
		if(this.turnningStatus==this.TURNNING_RIGHT&&this.turn==(-1*this.TURN_INCREASE)){
			this.turn=0;
			this.turnningStatus = !this.TURNNING_RIGHT;
		}else{
			this.turn+=(this.step>180)?0:this.TURN_INCREASE;
		}
		
	}
	public void turnRight(){
		if(this.turnningStatus!=this.TURNNING_RIGHT&&this.turn==this.TURN_INCREASE){
			this.turn=0;
			this.turnningStatus = this.TURNNING_RIGHT;
		}else{
			this.turn-=(this.step<-180)?0:this.TURN_INCREASE;
		}
	}
}
